
#include "MoveAlgorithm.h"

#define PI 3.14159265358979323846

const int PixelSize = 120; // 垂直方向像素数
const int ScreenSize = 50; // 屏幕是 50 * 50，单位是 cm
const int EquipmentDistance = 100; // 设备距离屏幕 100 cm

int* PixelConversion(int nextpixelx, int nextpixely)
{
	int* RealCoordinates = (int*)malloc(2 * sizeof(int)); // 开辟空间
	if (RealCoordinates == NULL) exit(1);	// 处理异常

	float ConversionRatio = (float)ScreenSize / (float)PixelSize; // 确定转换比例
	RealCoordinates[0] = (int)((float)nextpixelx * ConversionRatio);
	RealCoordinates[1] = (int)((float)nextpixely * ConversionRatio);

	return RealCoordinates;
}

float* MoveAlgorithm(enum LaserColor color, int nextpixelx, int nextpixely) // 传入的 x，y 均为 320 * 240 的像素坐标，左上角为原点
{
	int xPlusOrMinus = 1, xAdjust = 0, yAdjust = 0;
	int* RealCoordinates = PixelConversion(nextpixelx, nextpixely);

	if (color == RED)
	{
		// 调整角度使得 x 增加的方向为正，使得 y 增加的方向为正，红色激光笔位于 (25, 0)
		if (25 - RealCoordinates[0] < 0) xPlusOrMinus = -1;
		// 计算相较于中心 (25, 0) 要移动的距离
		xAdjust = abs(25 - RealCoordinates[0]);
		yAdjust = abs(RealCoordinates[1]);
	}
	else
	{
		// 调整角度使得 x 增加的方向为正，使得 y 增加的方向为正，绿色激光笔位于 (75, 0)
		if (75 - RealCoordinates[0]< 0) xPlusOrMinus = -1;
		// 计算相较于中心 (75, 0) 要移动的距离
		xAdjust = abs(75 - RealCoordinates[0]);
		yAdjust = abs(RealCoordinates[1]);
	}

	// 计算终点的 tan 角度
	float tanx = (float)xAdjust / (float)EquipmentDistance;
	float tany = (float)yAdjust / (float)EquipmentDistance;
	// 反正切函数，转为弧度制
	float radx = atan(tanx);
	float rady = atan(tany);
	// 弧度制转为角度制
	float degx = radx * 180.0 / PI;
	float degy = rady * 180.0 / PI;
	//返回结果
	free(RealCoordinates); // 释放空间
	float* EndPointAngle = (float*)malloc(2 * sizeof(float));
	if (EndPointAngle == NULL) exit(1);	// 处理异常
	EndPointAngle[0] = degx * (float)xPlusOrMinus + (float)90; // 决定角度的正负性
	EndPointAngle[1] = degy;

	return EndPointAngle;
}
